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ABSTRACT 

Accurate  navigation  information  (position,  velocity, 
and  attitude)  can  be  determined  using  optical  mea¬ 
surements  from  imaging  sensors  combined  with  an 
inertial  navigation  system.  This  can  be  accomplished 
by  tracking  the  locations  of  stationary  optical  features 
in  multiple  images  and  using  the  resulting  geometry 
to  estimate  and  remove  inertial  errors. 

In  previous  research  efforts,  we  have  demonstrated 
the  effectiveness  of  fusing  imaging  and  inertial  sen¬ 
sors  using  an  extended  Kalman  filter  (EKF)  algo¬ 
rithm.  In  this  approach,  the  image  feature  corre¬ 
spondence  search  was  aided  using  the  inertial  sen¬ 
sor  measurements,  resulting  in  more  robust  feature 
tracking.  The  resulting  image-aided  inertial  algo¬ 
rithm  was  tested  using  both  simulation  and  experi¬ 


mental  data.  While  the  tightly-coupled  approach  sta¬ 
bilized  the  feature  correspondence  search,  the  over¬ 
all  problem  remained  prone  to  filter  divergence  due 
to  the  well-known  consequences  of  image  scale  ambi¬ 
guity  and  the  nonlinear  measurement  model.  These 
effects  are  evidenced  by  the  consistency  divergence 
in  the  EKF  implementation  seen  during  our  long- 
duration  Monte-Carlo  simulations.  In  other  words, 
the  measurement  model  is  highly  sensitive  to  the  cur¬ 
rent  parameter  estimate,  which  invalidates  the  lin¬ 
earized  measurement  model  assumed  by  the  EKF. 

The  unscented  (sigma-point)  Kalman  filter  (UKF)  has 
been  proposed  in  the  literature  in  order  to  address 
the  large  class  of  recursive  estimation  problems  which 
are  not  well-modeled  using  linearized  dynamics  and 
Gaussian  noise  models  assumed  in  the  EKF.  The  UKF 
leverages  the  unscented  transformation  in  order  to 
represent  the  state  uncertainty  using  a  set  of  carefully 
chosen  sample  points.  This  approach  maintains  mean 
and  covariance  estimates  accurate  to  at  least  second 
order,  by  using  the  true  nonlinear  dynamics  and  mea¬ 
surement  models. 

In  this  paper,  a  variation  of  the  UKF  is  applied  to 
the  image-aided  inertial  navigation  problem,  with  the 
goal  of  improving  upon  the  established  limitations  of 
our  previous  EKF  implementation.  A  tightly-coupled 
image-aided  inertial  UKF  is  rigorously  designed  from 
first  principles.  The  UKF  is  evaluated  using  a  combi¬ 
nation  of  simulated  and  experimental  data.  The  per¬ 
formance  of  the  image-aided  navigation  system  is  an¬ 
alyzed  and  compared  to  the  baseline  EKF  from  our 
previous  work. 
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INTRODUCTION 

Motivation 

The  fact  that  GPS  signals  are  not  available  in  all  loca¬ 
tions  is  a  significant  issue  for  many  users  and  requires 
the  development  of  a  non-GPS  based  navigation  ref¬ 
erence  that  can  provide  long-term  accuracy  with  rea¬ 
sonable  cost.  In  previous  research,  the  benefits  of 
tightly-coupling  navigation  sensors  has  been  well- 
documented  and  can  be  seen  when  integrating  iner¬ 
tial  measurement  units  (IMU)  and  Global  Position¬ 
ing  System  (GPS)  measurements.  The  complimentary 
characteristics  of  the  two  sensors  allow  the  integrated 
system  to  perform  at  levels  which  are  much  better 
than  the  levels  attained  by  using  either  sensor  alone 
(see  [2]).  Consequently,  integrated  systems  have  be¬ 
come  very  popular,  especially  in  military-grade  navi¬ 
gation  systems. 

The  availability  issues  with  GPS-based  navigation 
can  be  addressed  by  a  non-GPS  navigation  approach 
which,  in  a  similar  manner,  seeks  to  couple  the  imag¬ 
ing  and  inertial  sensors  at  a  deep  level  [1, 11, 13].  This 
technique  has  some  important  advantages.  The  sen¬ 
sors  can  operate  in  a  broad  range  of  environments 
(e.g.,  indoors,  under  trees,  underwater,  etc.),  where 
GPS  might  not  be  available.  Secondly,  passive  signals 
are  used,  so  they  do  not  require  the  transmission  (or 
reception)  of  radio  signals.  As  a  result,  optical  and  in¬ 
ertial  sensors  are  immune  to  disruptions  in  the  radio 
spectrum. 

In  previous  work,  a  method  using  an  extended 
Kalman  filter  is  developed  to  tightly  integrate  optical 
and  inertial  sensors.  While  the  filter  performed  much 
better  than  the  inertial-only  solution,  the  overall  per¬ 
formance  suffered  from  periodic  filter  divergence  due 
to  linearization  errors  of  the  extended  Kalman  filter. 
These  effects  were  especially  prevalent  at  the  pres¬ 
ence  of  large  attitude  errors  [17].  This  paper  seeks 
to  address  these  issues  by  employing  the  unscented 
Kalman  filter  (UKF)  [5, 6],  which  has  been  shown  to 
address  these  concerns  with  minimal  increase  in  com¬ 
putation  time  and  algorithm  complexity.  In  the  next 
section,  an  overview  of  visual  navigation  techniques 
is  presented. 

Current  Methods 

The  advent  of  the  Global  Positioning  System  funda¬ 
mentally  changed  the  concept  of  precision  navigation 
for  navigators  who  have  traditionally  utilized  me¬ 
chanical  instruments  such  as  astrolabes,  sextants  and 
driftmeters  to  determine  their  position,  velocity  and 


orientation.  The  fact  that  GPS  can  not  be  used  in  all 
environments  forces  people  to  find  new  methods.  Ob¬ 
viously,  it  can  be  seen  that  there  is  a  synergy  between 
imaging  and  inertial  sensors  which  are  already  be¬ 
ing  used  by  human  or  other  animals.  This  synergy  is 
a  motivation  for  using  optical  measurements  to  pro¬ 
vide  high-quality  navigation  information. 

Using  optical  measurements  for  autonomous  pre¬ 
cision  navigation  is  dependent  upon  some  level 
of  computer-image  interpretation.  This  is  also  a 
difficulty  shared  with  Automatic  Target  Recogni¬ 
tion  (ATR).  Indeed,  the  ATR  problem  in  this  struc¬ 
tured  environment  is  tractable  using  celestial  obser¬ 
vations,  and  automatic  star  trackers  are  widely  used 
for  space  navigation  and  ICBM  guidance  [7,  9,  10]. 
When  surface  images  are  to  be  used,  the  difficulties 
associated  with  image  interpretation  are  paramount, 
however  at  the  same  time,  the  problems  associated 
with  the  use  of  optical  measurements  for  naviga¬ 
tion  are  somewhat  simpler  than  ATR.  These  dead¬ 
reckoning  image-based  navigation  techniques  require 
automated  methods  to  detect  and  track  optical  fea¬ 
tures  of  opportunity.  Moreover,  there  are  improve¬ 
ments  motivating  the  use  of  inertial  measurements 
to  aid  the  image  interpretation  such  as  recent  devel¬ 
opments  in  feature  tracking  algorithms,  miniaturiza¬ 
tion,  and  reduction  in  cost  of  inertial  sensors  and  op¬ 
tical  imagers  aided  by  the  continuing  improvement  in 
microprocessor  technology.  For  detailed  examples  of 
image-aiding  methods  see  [16]. 

A  rigorous,  stochastic  projection  algorithm  is  pre¬ 
sented  in  [18],  which  incorporates  inertial  measure¬ 
ments  into  a  predictive  feature  transformation,  ef¬ 
fectively  constraining  the  resulting  correspondence 
search  space.  The  algorithm  was  incorporated  into 
an  extended  Kalman  filter  and  tested  experimentally 
in  [17]  using  both  tactical  and  consumer  grade  iner¬ 
tial  sensors.  The  integrated  system  demonstrated  at 
least  two  orders  of  magnitude  improvement  over  the 
inertial-only  navigation  solution. 

One  nonlinear  filtering  approach  is  investigated  in 
this  paper.  In  order  to  improve  the  sub-optimal 
performance  of  the  extended  Kalman  filter,  the  un¬ 
scented  Kalman  filter  will  be  used.  In  the  EKF,  the 
state  distribution  is  approximated  by  a  jointly  Gaus¬ 
sian  random  vector  and  propagated  through  a  lin¬ 
earized  approximation  of  the  nonlinear  dynamics  and 
measurement  model.  Our  analysis  indicated  this  is 
the  reason  for  sub-optimal  performance  and  diver¬ 
gence  of  previous  work.  The  UKF  addresses  this  issue 
by  representing  the  state  distribution  as  a  collection 
of  sigma  points,  which  are  directly  transformed  us¬ 
ing  the  nonlinear  dynamics  and  measurement  mod- 


els.  This  has  been  shown  in  the  literature  to  preserve 
additional  moments  of  the  state  distribution  and,  as 
such,  is  more  resilient  to  the  deleterious  effects  of  lin¬ 
earization  errors.  In  the  following  section,  the  inte¬ 
gration  architecture  is  presented  in  detail. 

DEVELOPMENT 

The  unscented  Kalman  filter  algorithm  is  used  in 
this  research  to  recursively  estimate  the  navigation 
state  and  associated  errors.  As  in  previous  work, 
this  method  is  based  upon  automatically  tracking  the 
pixel  location  of  stationary  landmarks  in  an  image- 
aided  inertial  system.  The  system  is  designed  to  per¬ 
form  under  a  number  of  assumptions  which  are  listed 
below. 

Assumptions 

•  A  strapdown  inertial  measurement  unit  (IMU)  is 
rigidly  attached  to  one  or  more  cameras.  Syn¬ 
chronized  raw  measurements  are  available  from 
both  sensors. 

•  The  camera(s)  images  areas  in  the  environment 
which  contain  some  stationary  objects. 

•  Binocular  measurements  are  available  which 
provide  an  indication  of  range  to  objects  in  the 
environment. 

•  The  inertial  and  optical  sensors'  relative  position 
and  orientation  is  known.  See  [15]  for  a  discus¬ 
sion  of  boresight  calibration  procedures. 

The  navigation  state  consists  of  the  following  vari¬ 
ables,  shown  in  Table  1.  The  position  and  velocity 
components  are  three-dimensional  vectors  expressed 
relative  to  an  Earth-fixed  navigation  frame.  The 
orientation  is  expressed  using  a  body-to-navigation 
frame  direction  cosine  matrix  (DCM).  Because  of  the 
relatively  low-quality  of  the  MEMS  inertial  sensors, 
both  accelerometer  and  gyroscopic  biases  are  esti¬ 
mated  using  a  first-order  Gauss-Markov  model.  Fi¬ 
nally,  the  three-dimensional  position  of  each  land¬ 
mark  is  estimated  in  the  navigation  frame.  In  the  next 
section,  the  implementation  of  our  image-aided  iner¬ 
tial  navigation  system  using  the  UKF  is  presented. 

Algorithm  Description 

As  mentioned  previously,  the  unscented  Kalman  fil¬ 
ter  uses  a  deterministic  sampling  approach,  specifi¬ 
cally  the  unscented  transform  (UT)  in  which  the  state 
distribution  is  represented  using  a  minimal  set  of 


Table  1:  System  Navigation  State  Definition 


Parameter 

Description 

pn 

Vehicle  position  in  navigation  frame 
(northing,  easting,  and  down) 

vn 

Vehicle  velocity  in  navigation  frame 
(north,  east,  down) 

pn 

Vehicle  body  to  navigation  frame  DCM 

ab 

Acclerometer  bias  vector 

b6 

Gyroscope  bias  vector 

c 

Location  of  landmark  m  in  the 
navigation  frame  (one  for  each  landmark 
currently  tracked) 

carefully  chosen  sigma  points  around  the  mean.  The 
main  idea  is  to  approximate  a  Gaussian  distribution 
using  sample  points  instead  of  the  mean  and  covari¬ 
ance.  As  seen  in  the  block  diagram  of  the  system  in 
Figure  1,  an  unscented  Kalman  filter  is  designed  to  es¬ 
timate  the  errors  in  the  calculated  system  parameters 
(see  Table  1).  The  system  parameters  are  periodically 
corrected  in  order  to  maintain  the  best  possible  align¬ 
ment  for  the  INS  reference  trajectory. 


Figure  1:  Image-aided  inertial  navigation  filter  block  dia¬ 
gram.  In  this  filter,  the  location  of  stationary  objects  are 
tracked  and  used  to  estimate  and  update  the  errors  in  an 
inertial  navigation  system.  The  inertial  navigation  system 
is,  in  turn,  used  to  support  the  feature  tracking  loop. 

For  the  algorithm,  the  navigation  state  structure  is 
created  to  define  current  system  parameters.  Since 
the  INS  mechanization  equations  and  measurement 
equations  are  nonlinear,  the  unscented  transforma¬ 
tion  is  used  to  compute  a  collection  of  sigma  points  to 
represent  the  statistics  of  the  navigation  state  through 
nonlinear  transformations.  Before  using  a  nonlinear 
function  (in  the  propagation  and  update  cycles),  the 
current  navigation  state  errors  (dimension  L)  and  co- 
variance  matrix  are  used  to  generate  a  matrix  x  of 


2L  +  1  sigma  points. 

Xo  =  x  (1) 

Xi  =  x+(y/(L  +  A)PJ)j,  *G[1,L]  (2) 

Xi  =  x  —  +  A)P X)i-L,i  €  [L  +  1, 2Xr]  (3) 

where  x  is  the  mean  vector  of  navigation  state  errors, 
Px  is  the  covariance  of  the  navigation  state  mean 
error  vector,  ( \/(/,+  A)P.,:),  is  the  ith  column  of  the 
matrix  square  root  and  A  is  the  scaling  parameter. 
The  scaling  parameter  is  defined  by 

A  =  a2{L  +  k)  —  L  (4) 

where  a  is  a  constant  value  used  to  determine  the 
spread  of  sigma  points  (usually  set  to  1  <  a  <  10-4). 
Finally  n  is  the  secondary  scaling  parameter  (usually 
set  to  k  =  3  —  L )  [4]. 

While  the  spread  of  the  sigma  points  is  a  function 
of  the  error  distribution,  the  strapdown  mechaniza¬ 
tion  function  propagates  the  whole-valued  naviga¬ 
tion  state,  not  the  errors  (see  [12]).  Hence,  once  the 
collection  of  sigma  points  x,  is  computed  about  the 
nominal,  each  sigma  point  is  transformed  to  and  from 
whole-valued  navigation  state  sigma  points,  N,;,  us¬ 
ing  a  generalized  addition/subtraction  operation  de¬ 
fined  as: 

Ni  -  No  +  Xi  (5) 

=  Nj  —  N0  (6) 

For  parameters  in  Euclidian  space  (e.g.,  position,  ve¬ 
locity,  etc.),  the  addition  and  subtraction  operators  are 
simply  standard  vector  addition  and  subtraction.  Dif¬ 
ferencing  orientation  states,  namely  the  C'h‘  direction 
cosine  matrix,  is  based  on  the  notion  that  small  an¬ 
gular  changes  can  be  appropriately  represented  by 
a  simple  rotation  error  vector  (f  in  the  above  equa¬ 
tions),  whereas  this  is  not  true  for  general  orienta¬ 
tions.  This  property  is  exploited  in  many  navigation 
state  error  models  such  as  the  well-known  Pinson  er¬ 
ror  model,  which  represents  angular  errors  as  a  three- 
dimensional  error  vector  about  some  nominal  orien¬ 
tation  DCM  or  quaternion.  This  method  is  both  ele¬ 
gant  and  efficient. 

The  relative  navigation  state  to  error  model  equations 
are 


Sp? 

=  P?-PS 

(7) 

S< 

= 

(8) 

C^ni 

=  (J-^)  C”° 

(9) 

Ssf 

=  ai  —  ao 

(10) 

Sbt 

=  b;  “bo 

(11) 

K 

±.n  ±.n 

~  Zij  M  j 

(12) 

where  i/y  is  the  angular  error 

Ipn 

Vh  =  Ipe  (13) 

-  ^  Ji 

which  corresponds  to  the  i-th  sigma  point. 

Thus,  given  a  collection  of  angular  difference  sigma 
points,  the  whole-valued  body-to-navigation  frame 
DCMs  can  be  calculated  using  the  following  steps: 

•  Convert  the  angular  errors  to  the  equivalent  di¬ 
rection  cosine  matrix  [14].  This  represents  the 
orientation  error  from  the  estimated  navigation 
frame  to  the  nominal  navigation  frame. 

•  Multiply  this  DCM  with  the  nominal  body-to- 
navigation  frame  DCM. 

This  results  in  the  whole-valued  body-to-navigation 
frame  DCM  sigma  points,  represented  by  Crf‘. 

After  the  equivalent  whole-valued  sigma-points  are 
calculated,  they  are  propagated  through  the  dynam¬ 
ics  model,  which  is  in  this  case  the  strapdown  mech¬ 
anization  equation  [12, 14]. 

Nj  (4)  =  f  [Nj  (4_i) ,  uk,  wfc]  (14) 

where  uk  are  the  inertial  measurements  and  wk  is  the 
dynamics  noise  vector.  This  results  in  a  collection  of 
predicted  whole-valued  states  at  the  time  of  the  next 
image  (see  Figure  2). 


Figure  2:  Whole-valued  navigation  state  structures  are 
generated  using  sigma  points.  These  whole-valued  states 
are  then  propagated  through  the  strapdown  mechanization 
algorithm.  Finally,  the  differences  betzveen  each  predicted 
navigation  state  are  found  by  comparing  to  the  nominal, 
whole-valued  navigation  state. 

Given  this  collection  of  whole-valued  navigation  state 
sigma  points,  the  representative  statistics  (i.e.,  mean 
and  covariance)  can  be  calculated.  First,  the  differ¬ 
ence  between  each  predicted  whole-valued  naviga¬ 
tion  state  sigma  point  is  calculated  about  the  nomi¬ 
nal  navigation  state  sigma  point  using  the  approach 


outlined  in  equations  (6)  and  (7-12).  Once  the  repre¬ 
sentative  error  sigma  points  are  determined,  the  error 
state  mean  and  covariance  are  approximated  using 
the  weighted  average  and  weighted  outer  product 

2  L 

&K  =  1  (15) 

2= 0 
2  L 

pk  =  ^ZwiC)iXi,k\k-i^6K) 

2=0 

iXi,k\k-i  ~  s*k)T  +  Qd,  (16) 

where  Q,;  is  the  process  noise.  The  weights  are  given 

by 

w0(m)  =  (17) 

wt]  =  YT\+1~a2  +  /3  (18) 

w^m)  =  wic)  =  ^^xy  *e[i,2L]  (19) 

where  (3  is  a  weighting  parameter  used  to  incorporate 
any  prior  knowledge  of  the  distribution  of  x.  For  a 
Gaussian  distribution,  /3  =  2  has  been  shown  to  be 
optimal  [4]. 

The  propagated  whole-valued  navigation  state  sigma 
points  can  now  be  used  to  predict  the  measurement 
sigma  points  using  the  measurement  equation 

z i(4)  =  h  [N,  (tfc) ,  vfc]  (20) 

where  z j(ffc)  is  the  collection  of  predicted  fea¬ 
ture  space  locations  corresponding  to  the  currently 
tracked  landmarks.  An  illustration  of  this  prediction 
is  shown  in  Figure  3. 

Calculating  the  statistics  of  the  prediction  is  accom¬ 
plished  in  a  similar  manner  as  with  the  navigation  er¬ 
rors.  The  relevant  statistics  are  calculated  using  the 
following  weighted  sum 

2  L 

Zfc  =  £Wt(m)zt(4)  (21) 

2  =  0 
2  L 

p zk,zk  =  Wj  [zj(ffc)— Zfc] 

i= 0 

•  [Zj(ffc)  -  Zj]T  +  Rfc  (22) 

where  z j(tfc)  is  the  matrix  of  predicted  feature  loca¬ 
tions  at  time  tk. 

An  important  characteristic  of  the  tightly-coupled 
stochastic  feature  tracking  algorithm  is  the  ability 
to  restrict  the  feature  correspondence  search  space 


based  upon  the  predicted  feature  locations.  This  is  ac¬ 
complished  by  filtering  the  collection  of  measured  fea¬ 
ture  locations.  Measured  features  that  exceed  a  sta¬ 
tistical  Mahalanobis  distance  are  excluded  from  the 
correspondence  search.  In  this  paper,  a  2 -a  threshold 
was  applied.  If  a  statistically-unique  correspondence 
set  can  be  determined,  the  collection  of  measured  fea¬ 
ture  locations,  z (tk),  is  then  used  to  complete  the  UKF 
update  as  follows: 

2  L 

P  xh,xh  =  ^WlC\xitk  |fc-l-<5Xfc) 


2=0 

•  [Zj(tfc)  -Zfe]T  (23) 

Kk  =  PXk,ZkP~k\Zk  (24) 

=  5x.k  +  Kfe  [z (tk)  -  z k\  (25) 
P+  =  P^-KfeP2fc,,fcK^  (26) 


Finally,  the  estimated  error  is  removed  from  the  nom¬ 
inal  navigation  state  and  the  update  is  completed. 

A  conceptual  summary  of  the  propagation  and  mea¬ 
surement  cycles  for  the  unscented  Kalman  filter  is  as 
follows: 

•  Based  on  the  current  nominal  navigation  state 
estimate,  calculate  a  collection  of  whole-valued 
navigation  states  corresponding  to  the  calculated 
sigma  points. 

•  Propagate  the  set  of  whole-valued  navigation 
state  sigma  points  through  the  nonlinear  strap- 
down  mechanization  function. 

•  Calculate  the  pre-measurement  statistics  using 
the  weighted  sum  of  the  differences  between 
whole-valued  navigation  states. 

•  Predict  the  feature  locations  of  the  current  land¬ 
mark  tracks. 

•  Determine  a  statistical  correspondence  between 
the  predicted  and  measured  feature  locations. 

•  Calculate  the  statistics  of  the  predicted  feature  lo¬ 
cation  errors.  Use  the  measurement  residual  and 
Kalman  gain  to  correct  the  nominal  navigation 
state. 

•  Repeat  as  necessary. 

SYSTEM  TESTS 

As  in  the  previous  paper,  the  UKF-based  imaging  and 
inertial  fusion  navigation  algorithm  is  evaluated  us¬ 
ing  both  simulated  and  experimental  ground  profiles. 


The  profiles  are  designed  to  provide  a  range  of  image 
types  in  order  to  exercise  the  feature  tracking  algo¬ 
rithm.  The  simulation  and  results  are  presented  in 
the  next  section. 

The  data  collection  system  consisted  of  a  consumer- 
grade  MEMS  IMU  and  two  digital  cameras  (see  Fig¬ 
ure  4).  The  IMU  was  a  Crista  consumer-grade  MEMS 
unit  (see  [3])  which  measured  acceleration  and  an¬ 
gular  rate  at  100  Hz.  The  digital  cameras  were  both 
Pixelink  A-741  machine  vision  cameras  which  incor¬ 
porated  a  global  shutter  feature  and  a  Firewire  inter¬ 
face.  The  lenses  were  wide-angle  Pentax  lenses  with 
approximately  90  degrees  field  of  view.  The  sensors 
were  mounted  on  an  aluminum  plate  and  calibrated 
using  procedures  similar  to  those  described  in  [15]. 
Images  were  captured  at  approximately  2.5  Hz.  Al¬ 
though  this  sensor  was  not  used  for  this  paper,  a  Hon¬ 
eywell  HG1700  tactical-grade  inertial  measurement 
unit  was  co-mounted  on  the  platform  in  order  to  pro¬ 
vide  a  one-to-one  performance  comparison  between 
different  grades  of  inertial  sensors. 

Simulation 

The  algorithm  was  tested  using  a  Monte  Carlo  simu¬ 
lation  of  a  standard  indoor  profile.  The  profile  con¬ 
sisted  of  a  straight  corridor,  designed  to  be  similar  to 
the  indoor  experimental  data  collection. 

An  accurate  simulation  of  the  navigation  environ¬ 
ment  requires  simulating  the  performance  of  the  sen¬ 
sors  in  response  to  a  true  motion  trajectory.  The  trajec¬ 
tory  was  generated  using  ProfGen  version  8.19  soft¬ 
ware  package  [8].  For  each  Monte  Carlo  navigation 
simulation  run,  the  inertial  sensor  measurements  are 
regenerated  using  the  true  trajectory  and  an  inertial 
sensor  error  model. 

Because  of  the  inherent  complexity  of  the  optical  envi¬ 
ronment,  it  is  beyond  the  scope  of  this  paper  to  gener¬ 
ate  simulated  images.  Instead,  a  simulated  feature  set 
was  created  by  randomly  distributing  features  along 
a  corridor  surrounding  the  true  trajectory.  The  fea¬ 
tures  are  each  given  random  descriptor  vectors  in  or¬ 
der  to  exercise  the  feature  tracking  algorithm.  While 
this  optical  simulation  method  is  appropriate  for  test¬ 
ing  the  image  and  inertial  fusion  algorithm,  the  re¬ 
sults  are  not  directly  comparable  to  the  real  system 
performance,  because  imaging  issues  such  as  lighting 
conditions,  motion  blur,  and  affine  changes  in  the  fea¬ 
ture  descriptor  due  to  pose  changes  are  not  modeled. 
In  other  words,  these  results  are  optimistic  with  re¬ 
spect  to  position  error,  however  the  error  divergence 
trends  should  still  be  observable. 


Figure  3:  Stochastic  feature  projection.  Optical  features 
of  interest  are  projected  into  future  images  using  inertial 
measurements  and  stochastic  projections. 


Figure  4:  Data  collection  system.  The  data  collection 
system  consisted  of  a  consumer-grade  MEMS  IMU  and 
monochrome  digital  cameras.  Although  not  used  in  this 
paper,  a  tactical-grade  IMU  was  co-mounted  on  the  plat¬ 
form  in  order  to  provide  a  performance  comparison  between 
different  grades  of  inertial  sensors. 


The  simulated  corridor  was  3  meters  wide,  3  meters 
high,  and  approximately  300  meters  long.  Features 
were  randomly  generated  on  the  walls,  floor  and  ceil¬ 
ing  of  the  corridor  with  an  average  spacing  of  0.25 
features  per  square  meter.  Each  feature  was  given  a 
random  primary  length  and  orientation,  which,  com¬ 
bined  with  the  true  pose  of  the  sensor,  resulted  in  ac¬ 
curately  simulated  scale  and  orientation  parameters 
in  feature  space.  After  a  60-second  stationary  align¬ 
ment,  the  sensor  platform  accelerated  to  0.5  meters 
per  second,  maintained  this  velocity  until  the  end  of 
the  corridor,  then  accelerated  to  a  stop  at  the  end. 
The  platform  remained  stationary  for  60  seconds  after 
coming  to  a  stop.  This  resulted  in  a  660-second  image 
and  inertial  navigation  profile.  Simulated  images  are 
collected  at  2  Hz. 

A  Monte  Carlo  simulation  was  conducted  using 
an  inertial  sensor  model  representing  the  Crista 
consumer-grade  IMU.  Each  simulation  consisted  of 
60  runs,  each  with  randomly  generated  inertial  mea¬ 
surement  errors  due  to  random  walks,  sensor  bias, 
and  sensor  scale-factor  errors.  In  order  to  mitigate 
any  potential  effects  due  to  the  location  of  the  fea¬ 
tures  in  the  simulated  environment,  the  feature  loca¬ 
tions  and  descriptors  were  randomly  generated  every 
20  runs. 

The  simulated  position  errors  for  the  EKF  and  UKF 
are  shown  in  Figures  5  and  6,  respectively.  Significant 
excursions  in  position  are  noted  in  the  EKF-based  al¬ 
gorithm,  which  evidence  the  effects  of  increased  atti¬ 
tude  errors  resulting  in  destabilizing  linearization  er¬ 
rors.  In  contrast,  the  UKF-based  estimator  appears 
to  eliminate  the  departures  and  is  reasonably  consis¬ 
tent  with  the  estimated  uncertainty.  The  UKF  esti¬ 
mate  does,  however,  appear  to  be  biased.  This  is  not 
completely  unexpected  as  the  unscented  transforma¬ 
tion  can  be  shown  to  produce  a  biased  estimate  under 
non-symmetric  nonlinearities.  In  either  case,  the  ef¬ 
fects  are  relatively  small  and  should  be  "in-the-noise" 
when  processing  real  data  sets. 

In  order  to  investigate  this  issue  further,  the  velocity 
and  attitude  error  plots  are  shown  in  Figures  7  and  8, 
respectively.  The  velocity  errors  appear  to  be  very 
consistent  and  stable.  The  attitude  errors  show  a  dif¬ 
ferent  story,  especially  in  heading  error.  The  obvious 
heading  error  bias  explains  the  resulting  position  er¬ 
ror  bias.  Unfortunately,  the  cause  for  this  heading  er¬ 
ror  bias  is  unknown  and  will  require  further  investi¬ 
gation.  As  mentioned  previously,  the  apparent  stabil¬ 
ity  of  the  UKF-based  algorithm  should  outweigh  the 
effects  of  small  heading  bias  for  real  data  seta. 
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Figure  5:  Simulated  60-run  Monte  Carlo  position  error  re¬ 
sults /or  indoor  profile  with  a  consumer-grade  inertial  sen¬ 
sor  using  an  EKF-based  image  aiding  algorithm.  The  ex¬ 
tended  Kalman  filter  algorithm  displays  a  tendency  toward 
divergence  due  to  the  cumulative  effects  of  linearization  er¬ 
rors. 
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Figure  6:  Simulated  60-run  Monte  Carlo  position  error  re¬ 
sults  for  indoor  profile  with  a  consumer-grade  inertial  sen¬ 
sor  using  a  UKF-based  image  aiding  algorithm.  The  UKF- 
based  algorithm  s hows  no  indication  of  rapid  divergence, 
although  the  estimate  appears  to  contain  a  bias. 


In  the  next  section,  the  experimental  data  collection 


profile  and  results  are  presented  and  compared  be¬ 
tween  the  EKF  and  UKF  algorithms. 


Figure  7:  Simulated  60-run  Monte  Carlo  velocity  error  re¬ 
sults  for  indoor  profile  with  a  consumer-grade  inertial  sen¬ 
sor  using  a  UKF-based  image  aiding  algorithm.  As  ex¬ 
pected,  the  velocity  estimates  appear  consistent  and  stable. 
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Figure  8:  Simulated  60-run  Monte  Carlo  attitude  error 
residts  for  indoor  profile  with  a  consumer-grade  inertial 
sensor  using  a  UKF-based  image  aiding  algorithm.  The 
UKF-based  attitude  estimates  appear  to  be  relatively  stable 
and  consistent.  The  source  of  the  heading  bias  is  unknown, 
however  this  is  most  likely  the  root  cause  of  the  position 
error  bias. 


Experiment 

The  algorithm  was  tested  experimentally  using  a 
closed-loop  ground  navigation  profile  designed  to  ex¬ 
amine  the  operation  of  the  feature  tracking  system 
in  a  real-world  environment  and  compare  the  perfor¬ 
mance  between  the  EKF  and  UKF  implementations. 
The  profile  consisted  of  a  closed  path  in  an  indoor  en¬ 
vironment.  The  path  began  and  ended  at  the  same 
location  and  orientation  in  the  Advanced  Navigation 
Technology  (ANT)  Center  laboratory,  at  the  Air  Force 
Institute  of  Technology.  The  data  collection  began 
with  a  10-minute  stationary  alignment  period.  After 
the  alignment  period,  the  sensor  was  moved  in  a  10- 
minute  loop  around  the  hallways  of  the  building.  No 
prior  knowledge  was  provided  to  the  algorithm  re¬ 
garding  the  location  of  features  or  structure  of  the  en¬ 
vironment.  A  sample  image  from  the  indoor  profile  is 
shown  in  Figure  9. 


Figure  9:  Sample  image  from  indoor  data  collection.  The 
indoor  data  collection  presents  the  filter  with  man-made 
features  in  an  office  environment.  The  crosses  and  ellipses 
indicate  the  locations  and  uncertainty  of  currently  tracked 
landmarks. 

The  indoor  profile  presents  the  algorithm  with  dif¬ 
ferent  challenges  from  a  feature  tracking  perspective. 
The  indoor  environment  consists  of  repetitive,  visu¬ 
ally  identical  features  (e.g.,  floor  tiles,  lights,  etc.), 
which  can  easily  cause  confusion  for  the  feature  track¬ 
ing  algorithm.  In  addition,  reflections  from  win¬ 
dows  and  other  shiny  surfaces  might  not  be  inter¬ 
preted  properly  by  the  filter  and  could  potentially  re¬ 
sult  in  navigation  errors.  Finally,  the  lower  light  in¬ 
tensity  levels  and  large  areas  with  poor  contrast  (e.g., 
smooth,  featureless  walls)  presents  a  relatively  stark 


feature  space. 

The  filters'  estimates  of  the  trajectories  are  overlayed 
on  a  floor  plan  of  the  building  in  Figure  10  for  the 
EKF  and  UKF  algorithms.  For  both  EKF  and  UKF 
algorithms,  the  estimated  trajectory  generally  corre¬ 
sponds  to  the  building's  hallways,  with  excursions 
of  less  than  3  meters.  While  additional  testing  is  re¬ 
quired  to  fully  characterize  the  performance  of  the  al¬ 
gorithms,  the  navigation  accuracy  achieved  in  a  real- 
world  environment  indicates  promise  for  the  UKF- 
based  image-aided  inertial  navigation  system. 


Figure  10:  Estimated  trajectory  for  the  extended  Kalman 
filter  (blue)  and  unscented  Kalman  filter  (red)  image-aided 
inertial  algorithm.  Both  algorithms  demonstrate  similar 
performance,  within  the  expected  uncertainty  of  the  posi¬ 
tion  state  estimate. 


CONCLUSIONS 

Previous  research  presented  a  statistically  rigorous 
method  to  tightly  couple  imaging  and  inertial  sen¬ 
sors  using  an  extended  Kalman  filter.  Unfortunately, 
the  estimator  demonstrated  divergent  characteristics 
during  longer-term  navigation  profiled  which  was  at¬ 
tributed  to  the  cumulative  destabilizing  effects  of  lin¬ 
earization  errors.  To  address  this  known  weakness  of 
the  extended  Kalman  filter,  an  image-aided  naviga¬ 
tion  algorithm  based  on  the  unscented  Kalman  filter 
was  designed. 

The  filter  was  evaluated  and  compared  using  a  com¬ 
bination  of  simulated  and  experimental  data.  During 
the  evaluations,  the  stability  of  the  unscented  Kalman 
filter  was  shown  to  significantly  outperform  the  ex¬ 


tended  Kalman  filter.  In  addition,  the  unscented 
Kalman  filter  maintained  an  accurate,  and  consistent 
position  error  estimate.  One  potential  drawback  of 
the  UKF  for  this  application  was  the  presence  of  a 
systematic  bias  in  the  position  and  attitude  estimates. 
While  this  could  be  an  issue  for  some  applications,  we 
believe  the  greatly  improved  stability  performance 
greatly  outweighs  the  small  bias  effects  for  a  large  ma¬ 
jority  of  applications. 

DISCLAIMER 

The  views  expressed  in  this  article  are  those  of  the  au¬ 
thor  and  do  not  reflect  the  official  policy  or  position  of 
the  United  States  Air  Force,  Department  of  Defense, 
or  the  U.S  Government. 
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